import robomaster
from robomaster import robot
import time


def sub_position_handler(position_info):
    x, y, z = position_info
    # print("chassis position: x:{:.3f}, y:{:.3f}, z:{:.3f}".format(
    #     x*100, y*100, z*100))

global_angle = 0

def sub_angle_info_handler(angle_info):
    pitch_angle, yaw_angle, pitch_ground_angle, yaw_ground_angle = angle_info
    # print("gimbal angle: pitch_angle:{0}, yaw_angle:{1}, pitch_ground_angle:{2}, yaw_ground_angle:{3}".format(
    #     pitch_angle, yaw_angle, pitch_ground_angle, yaw_ground_angle))
    global global_angle
    global_angle = yaw_ground_angle

if __name__ == '__main__':
    ep_robot = robot.Robot()
    ep_robot.initialize(conn_type="sta")
    ep_robot.set_robot_mode(mode=robot.CHASSIS_LEAD)

    ep_chassis = ep_robot.chassis
    ep_gimbal = ep_robot.gimbal

    # 订阅底盘位置信息
    # ep_chassis.sub_position(freq=1, callback=sub_position_handler)
    # ep_chassis.move(x=1.0, y=1.0, z=90).wait_for_completed()
    ep_gimbal.sub_angle(freq=10,callback=sub_angle_info_handler)
    
    time.sleep(1)
    # while True:
    #     print("angle = {:.3f}".format(global_angle))
    #     s= input("pause")
    #     if s=='q':break
    
    print("angle = {:.3f}".format(global_angle))

    for base in range(10):
        # test_deg = (base+1)*30
        test_deg = 90
        print("check {}".format(test_deg))
        # L = []
        # R = []
        for _ in range(1):
            past_angle = global_angle
            ep_chassis.move(0,0,test_deg,0,90).wait_for_completed()
            end_angle = global_angle
            time.sleep(1)
            curr_angle = global_angle
            # L.append(curr_angle-past_angle)
            print("{:.3f} {:.3f}".format(end_angle - past_angle,curr_angle - past_angle))

        # for _ in range(5):
        #     past_angle = global_angle
        #     ep_chassis.move(0,0,-test_deg,0,90).wait_for_completed()
        #     time.sleep(1)
        #     curr_angle = global_angle
        #     R.append(curr_angle-past_angle)

        # print(L,R)
        # print("avg_L = {:.3f}".format(sum(L)/5))
        # print("avg_R = {:.3f}".format(sum(R)/5))

        # input("pause")

    time.sleep(2)
    # time.sleep(20)

    ep_gimbal.unsub_angle()
    # ep_chassis.unsub_position()

    ep_robot.close()